/*
 * @file base_server.cpp
 * @brief	base class with all the controls for the trunk/gripper.
 *
 * @author Michiel Hegemans
 * @author Peter Evers
 *
 * @date Jan 2013
 */

#include "servers/base_server.h"

/**
 * State received from the trunk.
 */
void BaseServer::trunkCallback(const comm::trunk::ConstPtr &msg)
{
	_trunk_state = msg->state;
}

/**
 * Callback of the gripper sensor.
 */
void BaseServer::sensorCallback(const std_msgs::Float32::ConstPtr &msg)
{
	_gripper_sensor = msg->data;
}

/**
 * Callack of the wheels.
 */
void BaseServer::wheelsCallback(const geometry_msgs::Twist::ConstPtr &msg)
{
	_wheels_speed = *msg;	// TODO: Gaat dit goed?
}

/**
 * Callback of the gripper.
 */
void BaseServer::gripperCallback(const comm::gripper::ConstPtr &msg)
{
	_gripper_state = msg->state;
}

void BaseServer::publishGripper(comm::gripper msg) {
	_gripper_pub.publish(msg);
}

/**
 * Publish message to the gripper.
 */
void BaseServer::publishTrunk(comm::trunk msg)
{
	_trunk_pub.publish(msg);
}

/**
 * Publish speed message to the wheels.
 */
void BaseServer::publishWheelsSpeed(geometry_msgs::Twist msg)
{
	_wheels_speed_pub.publish(msg);
}

/**
 * Publish the linear position to the wheels.
 */
void BaseServer::publishWheelsLinear(std_msgs::Float32 msg)
{
	_wheels_linear_pub.publish(msg);
}

/**
 * Publish the angular position to the wheels.
 */
void BaseServer::publishWheelsAngular(std_msgs::Float32 msg)
{
	_wheels_angular_pub.publish(msg);
}

/**
 * Return the state of the trunk.
 */
int BaseServer::getTrunkState()
{
	return _trunk_state;
}

/**
 * Return the state of the gripper.
 */
int BaseServer::getGripperState()
{
	return _gripper_state;
}

/**
 * Return speed of the wheels.
 */
geometry_msgs::Twist BaseServer::getWheelsSpeed()
{
	return _wheels_speed;
}

/**
 * Get the sensor distance.
 */
int BaseServer::getSensorDistance()
{
	return _gripper_sensor;
}
